/*
 * pid.c
 *
 *  Created on: 2021-09-27
 *      Author: chenss
 */

#include "pid.h"
#include "math_utils.h"

#include <math.h>

void PID_Init(PID_t *pid, PID_Type_t type, float P, float I, float D)
{
    assert_param(pid != NULL);

    memset(pid, 0, sizeof(*pid));
    pid->type = type;
    pid->lowerLimit = -1;
    pid->upperLimit = 1;
    pid->ramp = 0;

    switch(type)
    {
    case PID_Type_Positional:
        PID_Positional_Init(&pid->positional, P, I, D);
        break;

    case PID_Type_Incremental:
        PID_Incremental_Init(&pid->incremental, P, I, D);
        break;

    default:
        pid->type = PID_Type_Unknown;
        break;
    }
}

void PID_Set(PID_t *pid, float P, float I, float D)
{
    switch(pid->type)
    {
    case PID_Type_Positional:
        pid->positional.P = P;
        pid->positional.I = I;
        pid->positional.D = D;
        break;

    case PID_Type_Incremental:
        pid->incremental.arm_pid.Kp = P;
        pid->incremental.arm_pid.Ki = I;
        pid->incremental.arm_pid.Kd = D;
        arm_pid_init_f32(&pid->incremental.arm_pid, false);
        break;

    default:
        break;
    }
}

void PID_Get(PID_t *pid, float *pP, float *pI, float *pD)
{
    switch(pid->type)
    {
    case PID_Type_Positional:
        *pP = pid->positional.P;
        *pI = pid->positional.I;
        *pD = pid->positional.D;
        break;

    case PID_Type_Incremental:
        *pP = pid->incremental.arm_pid.Kp;
        *pI = pid->incremental.arm_pid.Ki;
        *pD = pid->incremental.arm_pid.Kd;
        break;

    default:
        break;
    }
}

void PID_SetLimit(PID_t *pid, float lower, float upper)
{
    pid->lowerLimit = lower;
    pid->upperLimit = upper;
}

void PID_SetRamp(PID_t *pid, float ramp)
{
    pid->ramp = fabsf(ramp);
}

float PID_Operate(PID_t *pid, float error)
{
    float out = 0;

    switch(pid->type)
    {
    case PID_Type_Positional:
        out = PID_Positional_Operate(&pid->positional, error, pid->lowerLimit, pid->upperLimit, pid->ramp);
        break;

    case PID_Type_Incremental:
        out = PID_Incremental_Operate(&pid->incremental, error, pid->lowerLimit, pid->upperLimit, pid->ramp);
        break;

    default:
        break;
    }

    return out;
}

void PID_Reset(PID_t *pid)
{
    switch(pid->type)
    {
    case PID_Type_Positional:
        PID_Positional_Reset(&pid->positional);
        break;

    case PID_Type_Incremental:
        PID_Incremental_Reset(&pid->incremental);
        break;

    default:
        break;
    }
}

float PID_Output(PID_t *pid)
{
    float out = 0;

    switch(pid->type)
    {
    case PID_Type_Positional:
        out = pid->positional.prevOutput;
        break;

    case PID_Type_Incremental:
        out = pid->incremental.prevOutput;
        break;

    default:
        break;
    }

    return out;
}

void PID_Positional_Init(PID_Positional_t *pid, float P, float I, float D)
{
    assert_param(pid != NULL);

    pid->P = P;
    pid->I = I;
    pid->D = D;

    pid->integral = 0;
    pid->prevError = 0;
    pid->prevOutput = 0;
    pid->prevTimestamp = 0;
}

float PID_Positional_Operate(PID_Positional_t *pid, float error, float lowerLimit, float upperLimit, float ramp)
{
    float integral = 0;
    float T = 1;
    Tick_t timestamp_now = Tick_Microseconds();

    if(pid->prevTimestamp != 0)
    {
        T = (timestamp_now - pid->prevTimestamp) * 1e-6f;
    }

#if 1
    if(pid->I > 1e-5)
    {
        integral = pid->integral + T * error / pid->I;
        integral = _constrain(integral, lowerLimit / pid->P, upperLimit / pid->P);
    }

    float derivative = pid->D * (error - pid->prevError) / T;

    float output = pid->P * (error + integral + derivative);
#else
    float proportional = pid->P * error;

    integral = pid->integral + pid->I*error*T;
    integral = _constrain(integral, lowerLimit, upperLimit);

    float derivative = pid->D * (error - pid->prevError) / T;

    float output = proportional + integral + derivative;
#endif
    if(ramp > 1e-2){
        float output_rate = (output - pid->prevOutput) / T;
        if (output_rate > ramp)
        {
            output = pid->prevOutput + ramp*T;
        }
        else if (output_rate < -ramp)
        {
            output = pid->prevOutput - ramp*T;
        }
    }

    output = _constrain(output, lowerLimit, upperLimit);
    pid->prevOutput = output;
    pid->integral = integral;
    pid->prevError = error;
    pid->prevTimestamp = timestamp_now;

    return output;
}

void PID_Positional_Reset(PID_Positional_t *pid)
{
    pid->integral = 0;
    pid->prevOutput = 0;
    pid->prevError = 0;
    pid->prevTimestamp = Tick_Microseconds();
}

void PID_Incremental_Init(PID_Incremental_t *pid, float P, float I, float D)
{
    pid->arm_pid.Kp = P;
    pid->arm_pid.Ki = I;
    pid->arm_pid.Kd = D;
    pid->prevOutput = 0;

    arm_pid_init_f32(&pid->arm_pid, true);
}

float PID_Incremental_Operate(PID_Incremental_t *pid, float error, float lowerLimit, float upperLimit, float ramp)
{
    float output = pid->prevOutput;

    output += arm_pid_f32(&pid->arm_pid, error);
    output = _constrain(output, lowerLimit, upperLimit);

    pid->prevOutput = output;

    return output;
}

void PID_Incremental_Reset(PID_Incremental_t *pid)
{
    pid->prevOutput = 0;

    arm_pid_reset_f32(&pid->arm_pid);
}
